% Devin Koepl

classdef AnimateActuatedModelClass < handle %(p, s, sp, t, x, y, ie, te, xe, fp)
    
    properties (Constant)
        t_step = 0.005; % 0.001;  
        
        % Length of the thigh for animation.
        thigh_length = 0.3;
        actuator_travel = 0.2;
        unstretched_spring_length = 0.5;

        theta = 0:0.05:2*pi;

        body_rx = 0.3;
        body_ry = 0.15;
        body_xy = [AnimateActuatedModelClass.body_rx * cos(AnimateActuatedModelClass.theta); ...
            AnimateActuatedModelClass.body_ry * sin(AnimateActuatedModelClass.theta)];

        hip_rx = 0.15;
        hip_ry = 0.03;
        hip_xy = [AnimateActuatedModelClass.hip_rx * cos(AnimateActuatedModelClass.theta); ...
            AnimateActuatedModelClass.hip_ry * sin(AnimateActuatedModelClass.theta)];
    end
    
    properties
        t;
        x;
        u;
        
        gnd;
        
        body_h;
    
        % Radius of the leg motor for animation.
        leg_mot;

        hip_h;

        %Create the leg, motors and desired hopping height as line objects.
        thigh;   
        hip_spr;
        shin;
        shin_spr;  
        ground;   
    end  
    
    methods
        
        function obj = AnimateActuatedModelClass(gnd, output)
            obj.gnd = gnd;
            
            figure('name', 'Actuated Model', 'position', [200, 50, 800, 600]);
            xlim([-2, 2]);
            ylim([-0.5, 3]);
            axis equal;
            hold on;
            
            obj.body_h = patch(obj.body_xy(1, :), obj.body_xy(2, :), 'c', 'linewidth', 3);
    
            % Radius of the leg motor for animation.
            obj.leg_mot = rectangle('linewidth', 3, 'curvature', [1, 1], 'edgecolor', 'k', 'facecolor', 'm');

            obj.hip_h = patch(obj.hip_xy(1,:), obj.hip_xy(2,:), 'm', 'linewidth', 3);

            %Create the leg, motors and desired hopping height as line objects.
            obj.thigh = line('linewidth', 3, 'color', 'k');   
            obj.hip_spr = line('linewidth', 3, 'color', 'k');
            obj.shin = line('linewidth', 3, 'color', 'k');
            obj.shin_spr = line('linewidth', 3, 'color', 'k');  
            obj.ground = line('linewidth', 3, 'color', 'k');   
            
            obj.process_simulation_output(output);
        end
        
        function process_simulation_output(obj, output)
            obj.t = [];
            obj.x = [];
            obj.u = [];
            
            % First just concatenate the raw data into the animation
            % object.
            for i = 1 : size(output, 1)
                obj.t = [ obj.t ; output{i, 1} ];
                obj.x = [ obj.x ; output{i, 2} ];
                obj.u = zeros( size(obj.x) );
            end
                     
            % We are going to be doing interpolation, so we need unique time,
            % state, and output values.
            [obj.t, unique_ind] = unique(obj.t);
            obj.x               = obj.x(unique_ind, :);
            obj.u               = obj.u(unique_ind, :);           
        end
        
        function animate(obj)
          
            %Step through the data using a fixed time step.
            for t_anim = obj.t(1) : obj.t_step : obj.t(end)   
                %Interpolate the actuated model's position from the solver output and the toe
                %position.
                x_anim = interp1(obj.t, obj.x, t_anim);
                
                psi = ActuatedModelClass.PSI(x_anim');
                
                % Zero force leg length.
                rz = ActuatedModelClass.rl * x_anim(ActuatedModelClass.oli) / ActuatedModelClass.G;

                xlim([x_anim(ActuatedModelClass.xci) - 1.5, x_anim(ActuatedModelClass.xci) + 1.5]);

                leg_xy = [0, -ActuatedModelClass.rl, -ActuatedModelClass.rl, 0; ...
                2*ActuatedModelClass.rl, 2*ActuatedModelClass.rl + obj.actuator_travel - rz, ...
                -rz, - rz] + repmat([0; obj.unstretched_spring_length], 1, 4);

                R = [cos(psi - pi/2), cos(psi); ...
                    sin(psi - pi/2), sin(psi)];     
                leg_xy = repmat([x_anim(ActuatedModelClass.xci); x_anim(ActuatedModelClass.yci)], 1, size(leg_xy, 2)) +  R * leg_xy;
                leg_mot_xy = [x_anim(ActuatedModelClass.xci); x_anim(ActuatedModelClass.yci)] - R * [0; obj.thigh_length];
                %        
                [leg_spr_x_data, leg_spr_y_data] = spring_anim(leg_xy(:, end)', [x_anim(ActuatedModelClass.xti), x_anim(ActuatedModelClass.yti)], 0.1);
                [hip_spr_x_data, hip_spr_y_data] = spring_anim(x_anim([ActuatedModelClass.xci, ActuatedModelClass.yci]) + obj.hip_rx * [cos(x_anim(ActuatedModelClass.ohi) - pi/2), ...
                    sin(x_anim(ActuatedModelClass.ohi) - pi/2)], x_anim([ActuatedModelClass.xci, ActuatedModelClass.yci]) + 0.17*[cos(psi + pi), sin(psi + pi)], 0.025);

                %Calculate the rotation matrix from body coordinates to world
                %coordinates, and then rotate the hip motor patch using this rotation
                %matrix.
                R = [cos(x_anim(ActuatedModelClass.ohi) - pi/2), -sin(x_anim(ActuatedModelClass.ohi) - pi/2); ...
                    sin(x_anim(ActuatedModelClass.ohi) - pi/2), cos(x_anim(ActuatedModelClass.ohi) - pi/2)];
                hip_xy_anim = repmat([x_anim(ActuatedModelClass.xci); x_anim(ActuatedModelClass.yci)], 1, length(obj.body_xy)) + R * obj.hip_xy;

                R = [cos(x_anim(ActuatedModelClass.obi)), -sin(x_anim(ActuatedModelClass.obi)); ...
                sin(x_anim(ActuatedModelClass.obi)), cos(x_anim(ActuatedModelClass.obi))];
                body_xy_anim = repmat([x_anim(ActuatedModelClass.xci); x_anim(ActuatedModelClass.yci)], 1, length(obj.body_xy)) +  R * obj.body_xy;        

                %Set the positions of the leg, body, motor inertias and desired
                %height indicator line in the GUI.
                set(obj.body_h, 'xdata', body_xy_anim(1, :), 'ydata', body_xy_anim(2, :));
                set(obj.hip_h, 'xdata', hip_xy_anim(1,:) , 'ydata', hip_xy_anim(2,:));     
                set(obj.hip_spr, 'xdata', hip_spr_x_data, 'ydata', hip_spr_y_data);
                set(obj.leg_mot, 'position', [leg_mot_xy(1) - ActuatedModelClass.rl, leg_mot_xy(2) - ActuatedModelClass.rl, 2*ActuatedModelClass.rl, 2*ActuatedModelClass.rl]); 
                set(obj.thigh, 'xdata', [x_anim(ActuatedModelClass.xci), leg_mot_xy(1)], 'ydata', [x_anim(ActuatedModelClass.yci), leg_mot_xy(2)]);
                set(obj.shin, 'xdata', leg_xy(1, 2:4), 'ydata', leg_xy(2, 2:4));
                set(obj.shin_spr, 'xdata', leg_spr_x_data, 'ydata', leg_spr_y_data);                   

                xdomain = get(gca, 'xlim');
                xg = xdomain(1) : 1e-3 : xdomain(2);
                set(obj.ground, 'xdata', xg, 'ydata', ...
                    obj.gnd.ground_height(t_anim, xg, x_anim(ActuatedModelClass.xti), x_anim(ActuatedModelClass.yti)) );
                
                %Wait so that the GUI is refreshed.
%                 pause;
                pause(0.01);
            end
            
        end
        
    end
    
end